#ifndef OUTSIDE_NAVIGATOR_HPP
#define OUTSIDE_NAVIGATOR_HPP

#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <vector>
#include <memory>
#include <cmath> 

namespace outside_navigation {

    class OutsideNavigator : public rclcpp::Node {
    public:
        OutsideNavigator();

    private:

        // 该结构体用于存储找到的空隙信息，包含在机器人坐标系下的坐标
        struct Gap {
            double width;             // 空隙宽度 (m)
            double center_angle_rad;  // 空隙中心相对于机器人前方的角度 (弧度)
            double center_x_robot;    // 空隙中心在机器人坐标系下的 x 坐标
            double center_y_robot;    // 空隙中心在机器人坐标系下的 y 坐标
            double center_z_robot;    // 空隙中心在机器人坐标系下的 z 坐标 (通常为 0)
            int start_index;          // 空隙在激光数据数组中的起始索引
            int end_index;            // 空隙在激光数据数组中的结束索引
        };



        void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
        void controlLoop();

        // --- 核心导航逻辑 ---
        void computeOutsideNavigationCommand(double& linear_x, double& linear_y, double& angular_z);
        void moveTowardsTarget(double& linear_x, double& linear_y, double& angular_z, double target_x_robot, double target_y_robot);

        // 速度渐变
        void applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                    double target_linear_x, double target_linear_y, double target_angular_z);

        // --- 辅助函数 ---
        std::vector<double> getFrontRanges(const sensor_msgs::msg::LaserScan::SharedPtr scan);
        // std::vector<double> filterRangesByDistance(const std::vector<double>& ranges, double max_distance);
        std::vector<Gap> findGaps(const std::vector<double>& ranges, double min_gap_width, double obstacle_threshold);
        std::vector<std::pair<double, double>> convertRangesToPoints(const std::vector<double>& ranges, double angle_min, double angle_increment);

        void publishVelocityCommand(double linear_x, double linear_y, double angular_z);
        void publishStopCommand();

        // 订阅者和发布者
        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_vel_pub_;
        rclcpp::TimerBase::SharedPtr control_timer_;

        // 参数
        double max_linear_velocity_;
        double max_angular_velocity_; 
        double min_gap_width_;
        double obstacle_threshold_;
        double front_angle_range_;
        double front_angle_half_rad_;
        //double max_distance_to_front_wall_; 
        double position_tolerance_;
        double goal_tolerance_;
        double gate_platform_height_;

        double max_acceleration_;
        double max_angular_acceleration_;


        // 状态变量
        sensor_msgs::msg::LaserScan::SharedPtr latest_scan_;
        bool scan_received_ = false;


        // 速度平滑相关
        double prev_linear_x_ = 0.0;
        double prev_linear_y_ = 0.0;
        double prev_angular_z_ = 0.0; 
    };

} // namespace outside_navigation

#endif // OUTSIDE_NAVIGATOR_HPP